﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.IO;
using System.Text;
using UnityEngine;

namespace PathFind.Algo {
    public class AStarBLJps2 : MonoBehaviour, IaStarAlgo {
        private BinaryHeap<Node> _openListBh;
        private NodePool<Node> _pool;
        private BitArray GridData { get; set; }
        private byte[] TestedGrid { get; set; }
        public short GridHeight { get; private set;}
        public short GridWidth { get; private set; }

        private Coordinate startPos;
        private Coordinate endPos;
        private int startIndex;
        private int endIndex;
        private short endX, endY;
        private short[] eXSpace = new short[2];
        private short[] eYSpace = new short[2];
        private bool preprocessedData;
        private List<Coordinate> tempSol = new List<Coordinate>();
        private List<Coordinate> tempSol2 = new List<Coordinate>();
        // xBoundaryPoints 存放边界的坐标值,下标为偶数下标的元素总是可走点,奇数下标的元素总是不可走点(0开始)
        public readonly List<List<short>> xBoundaryPoints = new List<List<short>>();
        // yBoundaryPoints 存放边界的坐标值,下标为偶数下标的元素总是可走点,奇数下标的元素总是不可走点(0开始)
        public readonly List<List<short>> yBoundaryPoints = new List<List<short>>();
        // 速记: 存放边界的坐标值
        // jumpLookup[0] 向南 (Item2 <= y <= Item1间的元素跳点都是(x, Item2)
        // jumpLookup[1] 向东 (Item1 <= x <= Item2间的元素跳点都是(Item2, y)
        // jumpLookup[2] 向北 (Item1 <= y <= Item2间的元素跳点都是(x, Item2)
        // jumpLookup[3] 向西 (Item2 <= x <= Item1间的元素跳点都是(Item2, y)
        public readonly List<List<Tuple<short, short>>>[] jumpLookup = new List<List<Tuple<short, short>>>[4];

        #region 初始化
        public AStarBLJps2() {
        }
        public AStarBLJps2(short width, short height) {
            ReInitData(width, height);
        }
        public AStarBLJps2(BitArray grid, short width, short height) {
            ReInitData(grid, width, height);
        }

        public void ReInitData(BitArray gd, short width, short height) {
            _openListBh = new BinaryHeap<Node>();
            _pool = new NodePool<Node>();
            if (gd.Count < width * height) {
                throw new Exception("grid.Count lesser than width*height");
            }

            GridData = gd;
            GridWidth = width;
            GridHeight = height;
            TestedGrid = new byte[(GridWidth * GridHeight+7) / 8];
        }
        public void ReInitData(short width, short height) {
            var bitArray = new BitArray(width * height, true);
            ReInitData(bitArray, width, height);
        }
        #endregion

        #region 格子检查方法
        public short GetHeight() {
            return GridHeight;
        }
        public short GetWidth() {
            return GridWidth;
        }
        public bool InBounds(int index) {
            return index < (GridHeight * GridWidth) && index >= 0;
        }

        public bool InBounds(Coordinate c) {
            return InBounds(GridIndex(c));
        }

        public bool InBounds(short x, short y) {
            return InBounds(GridIndex(x, y));
        }

        private int GridIndex(Coordinate c) {
            return GridIndex(c.x, c.y);
        }

        private int GridIndex(short x, short y) {
            if (x < 0 || x >= GridWidth || y < 0 || y >= GridHeight) {
                return -1;
            }

            return y * GridWidth + x;
        }

        private Coordinate IndexToCoordinate(in int index) {
            return new Coordinate((short) (index % GridWidth), (short) (index / GridWidth));
        }

        public bool IsCoordinateBlocked(Coordinate c) {
            return IsPassable(c);
        }

        public bool IsPassable(Coordinate c) {
            int index = GridIndex(c);
            return IsPassable(index);
        }

        public bool IsPassable(int index) {
            if (index == -1) {
                return false;
            }
            return GridData[index];
        }

        public bool IsPassable(short x, short y) {
            int index = GridIndex(x, y);
            return IsPassable(index);
        }

        private void SetChecked(int index) {
            int s = (index / 8);
            int yu = (index % 8);
            TestedGrid[s] = (byte) (TestedGrid[s] | (1 << yu));
        }

        private bool IsChecked(int index) {
            int s = index / 8;
            int yu = index % 8;
            return (TestedGrid[s] & (1 << (yu))) > 0;
        }
        bool CanEnter(Coordinate co, Direction dir, short roundStep) {
            return IsPassable(co.AddDirPlusRoundStep(dir, roundStep));
        }
        #endregion

        #region 空间检查方法
        // 算出x和y方向上,目标点的左右,上下边界,避免后续多次查找Boundary判断是否在目标点上!
        private void GetEndSpaceIds(short eX, short eY) {
            for (int i = 0; i < xBoundaryPoints[eY].Count - 1; i++) {
                var iX = xBoundaryPoints[eY][i];
                var i1X = xBoundaryPoints[eY][i + 1];
                if (iX <= eX && eX < i1X) {
                    eXSpace[0] = iX;
                    eXSpace[1] = i1X;
                    break;
                }
            }
            for (int i = 0; i < yBoundaryPoints[eX].Count - 1; i++) {
                var iY = yBoundaryPoints[eX][i];
                var i1Y = yBoundaryPoints[eX][i+1];
                if (iY <= eY && eY < i1Y) {
                    eYSpace[0] = iY;
                    eYSpace[1] = i1Y;
                }
            }
        }
        bool IsSpaceIdX(int spaceId, short spaceX, short spaceY) {
            if (spaceId == -1) {
                if (xBoundaryPoints[spaceY].Count > 1) {
                    return xBoundaryPoints[spaceY][0] > spaceX;
                }
                else
                    return true;
            }
            else if (xBoundaryPoints[spaceY][spaceId] <= spaceX && spaceX < xBoundaryPoints[spaceY][spaceId + 1]){
                return true;
            }
            return false;
        }
        bool IsSpaceIdY(int spaceId, short spaceX, short spaceY) {
            if (spaceId == -1) {
                if (yBoundaryPoints[spaceX].Count > 1) {
                    return yBoundaryPoints[spaceX][0] > spaceY;
                }
                else
                    return true;
            }
            else if (yBoundaryPoints[spaceX][spaceId] <= spaceY && spaceY < yBoundaryPoints[spaceX][spaceId + 1]) {
                return true;
            }
            return false;
        }
        int GetSpaceIdY(short spaceX, short spaceY)
        {
            for (int i = 0; i<yBoundaryPoints[spaceX].Count - 1; i++)
                if (yBoundaryPoints[spaceX][i] <= spaceY && yBoundaryPoints[spaceX][i + 1]>spaceY)
                    return i;
            return -1;
        }
        int GetSpaceIdX(short spaceX, short spaceY)
        {
            for (int i = 0; i<xBoundaryPoints[spaceY].Count - 1; i++)
                if (xBoundaryPoints[spaceY][i] <= spaceX && xBoundaryPoints[spaceY][i + 1]>spaceX)
                    return i;
            return -1;
        }
        #endregion

        #region 快捷方法主体
        private bool DirectSolution(short sX, short sY, short eX, short eY, ref List<Coordinate> solution) {
            if (sY == eY) {
                if (IsSpaceIdX(GetSpaceIdX(sX, sY), eX, eY)) {
                    solution.Add(new Coordinate(eX, eY));
                    solution.Add(new Coordinate(sX, sY));
                    return true;
                }
            }
            else if (sX == eX) {
                if (IsSpaceIdY(GetSpaceIdY(sX, sY), eX, eY)) {
                    solution.Add(new Coordinate(eX, eY));
                    solution.Add(new Coordinate(sX, sY));
                    return true;
                }
            }
            short absX = (short) (sX < eX ? eX - sX : sX - eX);
            short absY = (short) (sY < eY ? eY - sY : sY - eY);
            short offsetX = (short) (eX > sX ? 1 : -1);
            short offsetY = (short) (eY > sY ? 1 : -1);
            var isDiagOnly = absX == absY;
            var minAbs = absX > absY ? absY : absX;
            //we -1 as we don't need to check the original or destination points as they are checked earlier
            int diagMovements = minAbs - 1;
            bool bPass = true;
            if (!isDiagOnly) {
                Coordinate check2 = new Coordinate(
                    (short) (sX + offsetX * minAbs),
                    (short) (sY + offsetY * minAbs)
                );
                if (absX < absY) {
                    if (!IsSpaceIdY(GetSpaceIdY(check2.x, check2.y), eX, eY)) {
                        bPass = false;
                    }
                }
                else {
                    if (!IsSpaceIdX(GetSpaceIdX(check2.x, check2.y), eX, eY)) {
                        bPass = false;
                    }
                }
            }
            if (bPass) {
                short checkX = sX;
                short checkY = sY;
                for (int i = 0; i < diagMovements; i++) {
                    short newCheckX = (short) (checkX + offsetX);
                    short newCheckY = (short) (checkY + offsetY);
                    if (!IsPassable(newCheckX, newCheckY)) {
                        bPass = false;
                        break;
                    }
#if DIAG_UNBLOCKED
                    int index1 = GridIndex(newCheckX, checkY);
                    int index2 = GridIndex(checkX, newCheckY);
                    bool b = InBounds(index1) && IsPassable(index1) &&
                             InBounds(index2) && IsPassable(index2);
                    if (!b) {
                        bPass = false;
                        break;
                    }
#endif
                    checkX = newCheckX;
                    checkY = newCheckY;
                }
#if DIAG_UNBLOCKED
                if (bPass && minAbs != 0) {
                    int index1 = GridIndex((short)(checkX+offsetX), checkY);
                    int index2 = GridIndex(checkX, (short)(checkX+offsetX));
                    bPass = InBounds(index1) && IsPassable(index1) &&
                            InBounds(index2) && IsPassable(index2);
                }
#endif
                if (bPass) {
                    if (isDiagOnly) {
                        solution.Add(new Coordinate(eX, eY));
                        solution.Add(new Coordinate(sX, sY));
                        return true;
                    }
                    else {
                        solution.Add(new Coordinate(eX, eY));
                        solution.Add(new Coordinate((short)(checkX+offsetX), (short)(checkY+offsetY)));
                        solution.Add(new Coordinate(sX, sY));
                        return true;
                    }
                }
            }
            if (isDiagOnly) {
                return false;
            }
            // 如果从起点到终点查找失败,那么就从终点到起点找
            short mOffsetX = (short) (-offsetX);
            short mOffsetY = (short) (-offsetY);
            bPass = true;
            Coordinate check3 = new Coordinate(
                (short)(eX + mOffsetX * minAbs),
                (short)(eY + mOffsetY * minAbs));
            if (absX < absY) {
                if (!IsSpaceIdY(GetSpaceIdY(check3.x, check3.y), sX, sY)) {
                    bPass = false;
                }
            }
            else {
                if (!IsSpaceIdX(GetSpaceIdX(check3.x, check3.y), sX, sY)) {
                    bPass = false;
                }
            }
            if (bPass) {
                short checkX = eX;
                short checkY = eY;
                for (int i = 0; i < diagMovements; i++) {
                    short newCheckX = (short) (checkX + mOffsetX);
                    short newCheckY = (short) (checkY + mOffsetY);
                    if (!IsPassable(newCheckX, newCheckY)) {
                        bPass = false;
                        break;
                    }
#if DIAG_UNBLOCKED
                    int index1 = GridIndex(newCheckX, checkY);
                    int index2 = GridIndex(checkX, newCheckY);
                    bool b = InBounds(index1) && IsPassable(index1) &&
                             InBounds(index2) && IsPassable(index2);
                    if (!b) {
                        bPass = false;
                        break;
                    }
#endif
                    checkX = newCheckX;
                    checkY = newCheckY;
                }

                if (bPass) {
                    solution.Add(new Coordinate(eX, eY));
                    solution.Add(new Coordinate(
                        (short) (checkX + mOffsetX),
                        (short) (checkY + mOffsetY)));
                    solution.Add(new Coordinate(sX, sY));
                    return true;
                }
            }
            return false;
        }
        #endregion

        #region 算法主体
        public Coordinate[] FindSolution2(short sX, short sY, short eX, short eY) {
            // 没有重入的情况,避免重复创建坐标列表
            FindSolution(sX, sY, eX, eY, ref tempSol);
            tempSol2.Clear();
            if (SimplifySolutionStart(ref tempSol, ref tempSol2))
            {
                if (SimplifySolutionEnd(ref tempSol2, ref tempSol))
                {
                    return tempSol.ToArray();
                }
                else
                {
                    return tempSol2.ToArray();
                }
            }
            else
            {
                if (SimplifySolutionEnd(ref tempSol, ref tempSol2))
                {
                    return tempSol2.ToArray();
                }
                else
                {
                    return tempSol.ToArray();
                }
            }
        }
        public bool FindSolution(short sX, short sY, short eX, short eY, ref List<Coordinate> solution) {
            solution.Clear();
            endX = eX;
            endY = eY;
            endPos = new Coordinate(eX, eY);
            endIndex = GridIndex(eX, eY);
            startPos = new Coordinate(sX, sY);
            startIndex = GridIndex(sX, sY);
            if (sX == eX && sY == eY) {
                return true;
            }
            if (!InBounds(startIndex) || !InBounds(endIndex) || !IsPassable(startPos) || !IsPassable(endPos)) {
                return false;
            }

            if (DirectSolution2(sX, sY, eX, eY, ref solution)) {
                return true;
            }

            GetEndSpaceIds(eX, eY);
            _openListBh.Clear();
            Array.Clear(TestedGrid, 0, TestedGrid.Length);
            _pool.Reset();
            var startNode = _pool.GetNewNode(startPos, endPos, Direction.CENTER, null);
            _openListBh.Insert(startNode);
            SetChecked(GridIndex(startPos));
            DirectionSet dSet = new DirectionSet();
            while (_openListBh.Count() > 0) {
                dSet.Clear();
                Node currentNode = _openListBh.PopMin();
                AddForcedNeighbours(ref dSet, ref currentNode.pos, currentNode.dir);
                AddNaturalNeighbours(ref dSet, currentNode.dir);
                for (Direction dir = Direction.START; dir < Direction.END; dir++) {
                    if (!dSet.IsDirectionSet(dir)) {
                        continue;
                    }
                    int nextIndex = JumpNew(currentNode.pos, dir);
                    if (!InBounds(nextIndex)) {
                        continue;
                    }
                    if (!IsPassable(nextIndex)) {
                        SetChecked(nextIndex);
                        continue;
                    }
                    Coordinate nextC = IndexToCoordinate(nextIndex);
                    if (nextIndex == endIndex) {
                        solution.Add(endPos);
                        Node solutionNode = currentNode;
                        while (solutionNode != null) {
                            solution.Add(solutionNode.pos);
                            solutionNode = solutionNode.parent;
                        }
                        _openListBh.Clear();
                        dSet.Clear();
                        return true;
                    }

                    if (!IsChecked(nextIndex)) {
                        var newNode = _pool.GetNewNode(nextC, endPos, dir, currentNode);
                        _openListBh.Insert(newNode);
                        SetChecked(nextIndex);
                    }
                    else {
                        Node t = _pool.GetNewNode(nextC, endPos, dir, currentNode);
                        _openListBh.InsertSmaller(t);
                    }
                }
            }

            return false;
        }
        #endregion

        #region 跳点查找
        private int JumpNew(Coordinate currentNodePos, Direction dir) {
            var offset = dir.DirOffset();
            var nc = currentNodePos + offset;
#if DEBUG_GRID_SHOW
            // if (nc != startPos && nc != endPos && InBounds(nc) && IsPassable(nc)) {
            //     if (grid) {
            //         grid.ChangeColor(nc, GridColor.Cyan);
            //     }
            // }
#endif
            bool isDiag = dir.IsDiag();
            var dirAdd1 = dir.AddRoundStep(1);
            var dirAdd7 = dir.AddRoundStep(7);
            while (true) {
#if DIAG_UNBLOCKED
                if (isDiag) {
                    short x1 = (short) (nc.x - offset.x);
                    short y2 = (short) (nc.y - offset.y);
                    bool canThroughDiag = InBounds(x1, nc.y) && InBounds(nc.x, y2) &&
                                     IsPassable(x1, nc.y) && IsPassable(nc.x, y2);
                    if(!canThroughDiag) {
                        return -1;
                    }
                }
#endif
                if (!IsPassable(nc))
                    return -1;
                var index = GridIndex(nc);
                if (endIndex == index || HasForcedNeighbours(ref nc, dir)) {
                    return index;
                }
                if (isDiag) {
                    var newJpIndex = GetJumpPointNew(nc, dirAdd1);
                    if (newJpIndex > 0) {
                        // 注意:当走对角线时,nc查到跳点,是将nc放到最小堆里,所以返回的是index(
                        // 而不是newJpIndex)
                        return index;
                    }
                    newJpIndex = GetJumpPointNew(nc, dirAdd7);
                    if (newJpIndex > 0) {
                        return index;
                    }
                }
                else {
                    // 水平或垂直查找时，用当前的坐标作为起点(因为nc有可能就是我们要查找的跳点坐标)
                    return GetJumpPointNew(currentNodePos, dir);
                }
                // return JumpNew(nc, dir);
                nc = nc.AddDirOffset(dir);    // 用来走对角线的
            }
        }

        private int GetJumpPointNew(Coordinate c, Direction dirAdd) {
            var s = c.AddDirOffset(dirAdd);
            if (!IsPassable(s)) {
                return -1;
            }

            short newVal;
            switch (dirAdd) {
                case Direction.S:
                    // 将查找到的值存入 eYSpace[0] 和 eYSpace[1]
                    newVal = BinarySearchR(jumpLookup[0][s.x], s.y);
                    // 1.起点跟终点同一列
                    // 2.终点在起点南边
                    // 3.且 起点 和 终点 都在 范围内
                    if (s.x == endX && s.y >= endY &&
                        s.y >= eYSpace[0] && s.y <= eYSpace[1]
                                    ) {
                        return endIndex;
                    }
                    return GridIndex(s.x, newVal);
                case Direction.E:
                    newVal = BinarySearchL(jumpLookup[1][s.y], s.x);
                    if (s.y == endY && s.x <= endX &&
                        s.x <= eXSpace[1] && s.x>= eXSpace[0]) {
                        return endIndex;
                    }
                    return GridIndex(newVal, s.y);
                case Direction.N:
                    newVal = BinarySearchL(jumpLookup[2][s.x], s.y);
                    if (s.x == endX && s.y <= endY &&
                        s.y <= eYSpace[1] && s.y >= eYSpace[0]) {
                        return endIndex;
                    }
                    return GridIndex(s.x, newVal);
                case Direction.W:
                    newVal = BinarySearchR(jumpLookup[3][s.y], s.x);
                    if (s.y == endY&& s.x >= endX &&
                        s.x >= eXSpace[0] && s.x <= eXSpace[1]) {
                        return endIndex;
                    }
                    return GridIndex(newVal, s.y);
            }
            return -1;
        }

        public short BinarySearchL(List<Tuple<short, short>> v, short val) {
            short lo = 0;
            short hi = (short) (v.Count - 1);
            while (lo <= hi) {
                short mid = (short) (lo + (hi - lo) / 2);
                var lookupV = v[mid];
                if (val < lookupV.Item1) {
                    hi = (short) (mid - 1);
                }
                else if (val > lookupV.Item2) {
                    lo = (short) (mid + 1);
                }
                else {
                    return lookupV.Item2;
                }
            }
            return -1;
        }

        public short BinarySearchR(List<Tuple<short, short>> v, short val) {
            short lo = 0;
            short hi = (short) (v.Count - 1);
            while (lo <= hi) {
                short mid = (short) (lo + (hi - lo) / 2);
                var lookupV = v[mid];
                if (val < lookupV.Item2) {
                    hi = (short) (mid - 1);
                }
                else if (val > lookupV.Item1) {
                    lo = (short) (mid + 1);
                }
                else {
                    return lookupV.Item2;
                }
            }
            return -1;
        }

        // 老的跳点查找方法,在预处理的时候仍然需要用到
        private int RawJump(Coordinate currentNodePos, Direction dir) {
            Coordinate nc = currentNodePos.AddDirOffset(dir);
#if DIAG_UNBLOCKED
            if (dir.IsDiag()) {
                Coordinate nextC2 = currentNodePos.AddDirPreviousOffset(dir);
                Coordinate nextC3 = currentNodePos.AddDirNextOffset(dir);
                var canThroughDiag = InBounds(nextC2) && InBounds(nextC3) && IsPassable(nextC2)
                                 && IsPassable(nextC3);
                if(!canThroughDiag) {
                    return -1;
                }
            }
#endif
            while (true)
            {
                if (!IsPassable(nc)) {
                    return -1;
                }
                int index = GridIndex(nc);
                if (index == endIndex || HasForcedNeighbours(ref nc, dir)) {
                    return index;
                }
                if (dir.IsDiag()) {
                    int next = RawJump(nc, dir.AddRoundStep(7));
                    if (next >= 0) {
                        return index;
                    }

                    next = RawJump(nc, dir.AddRoundStep(1));
                    if (next >=0) {
                        return index;
                    }
                }
                nc = nc.AddDirOffset(dir);
            }
            return -1;
        }
        #endregion

        #region 邻居
        bool Implies(bool a, bool b) {
            return a ? b : true;
        }
        // 看 《Online Graph Pruning for Pathfnding on Grid Maps》 Figure2
        public void AddForcedNeighbours(ref DirectionSet neighSet, ref Coordinate pos, Direction dir) {
            if (dir >= Direction.CENTER) {
                return;
            }
            if (dir.IsDiag()) {
#if DIAG_UNBLOCKED
                // 是否需要加 CanEnter(pos, dir, 7) 这条件,要看对角线可走的定义:
                //     这里采用的是dir+1和dir-1至少有一个点可走时,dir才是可走的
                if (CanEnter(pos, dir, 6)
                    && (!CanEnter(pos, dir, 5) && CanEnter(pos, dir, 7))) {
                    neighSet.AddDirectionToSet(dir.AddRoundStep(6));
                }
                if (CanEnter(pos, dir, 2) &&
                    (!CanEnter(pos, dir, 3) && CanEnter(pos, dir, 1))) {
                    neighSet.AddDirectionToSet(dir.AddRoundStep(2));
                }
#else
                if (!Implies(CanEnter(pos, dir, 6) ,CanEnter(pos, dir, 5)))
                    neighSet.AddDirectionToSet(dir.AddRoundStep(6));
                if (!Implies(CanEnter(pos, dir, 2) ,CanEnter(pos, dir, 3)))
                    neighSet.AddDirectionToSet(dir.AddRoundStep(2));

#endif
            }
            else {
#if DIAG_UNBLOCKED
                // 2 可走 3 不可走的情况下
                // 这里采用这种格式是为了跟C++代码一致
                if (!Implies(CanEnter(pos, dir, 2), CanEnter(pos, dir, 3))) {
                    neighSet.AddDirectionToSet(dir.AddRoundStep(2));
                    if (CanEnter(pos, dir, 1)) {
                        neighSet.AddDirectionToSet(dir.AddRoundStep(1));
                    }
                }
                // 6 可走 5 不可走的情况下
                if (!Implies(CanEnter(pos, dir, 6), CanEnter(pos, dir, 5))) {
                    neighSet.AddDirectionToSet(dir.AddRoundStep(6));
                    if (CanEnter(pos, dir, 7)) {
                        neighSet.AddDirectionToSet(dir.AddRoundStep(7));
                    }
                }
#else
                if (!Implies(CanEnter(pos, dir, 7) ,CanEnter(pos, dir, 6)))
                    neighSet.AddDirectionToSet(dir.AddRoundStep(7));
                if (!Implies(CanEnter(pos, dir, 1) ,CanEnter(pos, dir, 2)))
                    neighSet.AddDirectionToSet(dir.AddRoundStep(1));
#endif
            }
        }
        public bool HasForcedNeighbours(ref Coordinate pos, Direction dir) {
            if (dir >= Direction.CENTER) {
                return true;
            }
            if (dir.IsDiag()) {
#if DIAG_UNBLOCKED
                if (CanEnter(pos, dir, 6)
                    && (!CanEnter(pos, dir, 5) && CanEnter(pos, dir, 7))) {
                    return true;
                }
                if (CanEnter(pos, dir, 2) &&
                    (!CanEnter(pos, dir, 3) && CanEnter(pos, dir, 1))) {
                    return true;
                }
#else
                if (!Implies(CanEnter(pos, dir, 6), CanEnter(pos, dir, 5)))
                    return true;
                if (!Implies(CanEnter(pos, dir, 2) ,CanEnter(pos, dir, 3)))
                    return true;
#endif
            }
            else {
#if DIAG_UNBLOCKED
                // 2 可走 3 不可走的情况下
                // 这里采用这种格式是为了跟C++代码一致
                if (!Implies(CanEnter(pos, dir, 2), CanEnter(pos, dir, 3))) {
                    return true;
                }
                // 6 可走 5 不可走的情况下
                if (!Implies(CanEnter(pos, dir, 6), CanEnter(pos, dir, 5))) {
                    return true;
                }
#else
                if (!Implies(CanEnter(pos, dir, 7) ,CanEnter(pos, dir, 6)))
                    return true;
                if (!Implies(CanEnter(pos, dir, 1) ,CanEnter(pos, dir, 2)))
                    return true;
#endif
            }
            return false;
        }
        public void AddNaturalNeighbours(ref DirectionSet neighSet, Direction dir) {
            if (dir >= Direction.CENTER) {
                neighSet.SetAll(true);
                return;
            }
            neighSet.AddDirectionToSet(dir);
            if (dir.IsDiag()) {
                neighSet.AddDirectionToSet(dir.AddRoundStep(1));
                neighSet.AddDirectionToSet(dir.AddRoundStep(7));
            }
        }
        #endregion

        #region 数据处理相关等方法
        public void PreProcessGrid() {
            preprocessedData = true;
            xBoundaryPoints.Clear();
            for (short y = 0; y < GridHeight; y++) {
                bool currentPassable = false;
                xBoundaryPoints.Add(new List<short>());
                for (short x = 0; x < GridWidth; x++) {
                    if (IsPassable(x, y) != currentPassable) {
                        xBoundaryPoints[y].Add(x);
                        currentPassable = !currentPassable;
                    }
                }
                xBoundaryPoints[y].Add(GridWidth);
            }
            yBoundaryPoints.Clear();
            for (short x = 0; x < GridWidth; x++) {
                bool currentPassable = false;
                yBoundaryPoints.Add(new List<short>());
                for (short y = 0; y < GridHeight; y++) {
                    if (IsPassable(x, y) != currentPassable) {
                        yBoundaryPoints[x].Add(y);
                        currentPassable = !currentPassable;
                    }
                }
                yBoundaryPoints[x].Add(GridHeight);
            }

            for (int i = 0; i < 4; i++) {
                jumpLookup[i] = new List<List<Tuple<short, short>>>();
            }
            for (short y = 0; y < GridHeight; y++) {
                var ls1 = new List<Tuple<short, short>>();
                var ls3 = new List<Tuple<short, short>>();
                jumpLookup[1].Add(ls1);
                jumpLookup[3].Add(ls3);
                // jumpLookup[1] 是从西到东方向的跳点lookup table,
                for (int xId = 0; xId < xBoundaryPoints[y].Count; xId+=2) {
                    // 下标为偶数元素都是第一个可走点
                    short x = xBoundaryPoints[y][xId];
                    int index;
                    do {
                        // 从第一个可走点向东,可以求出跳点,那么从可走点(x,y),到(newX, y)间的点的跳点都是(newX, y)
                        index = RawJump(new Coordinate(x, y), Direction.E);
                        if (index != -1) {
                            short newX = IndexToCoordinate(index).x;
                            ls1.Add(new Tuple<short, short>(x, newX));
                            x = newX;
                        }
                    } while (index != -1);
                }
                for (int xId = 1; xId < xBoundaryPoints[y].Count; xId += 2) {
                    // 因为:下标为奇数元素都是从西到东的第一个障碍物,由于xBoundaryPoints中边界值必然成对出现,
                    // 所以:第一个障碍物的前一个点,必然是可走点
                    short x = (short) (xBoundaryPoints[y][xId]-1);
                    int index;
                    do {
                        // 从可走点往西可以求出跳点,那么从(newX, y)到(x,y) 间的跳点都是(newX, y)
                        index = RawJump(new Coordinate(x, y), Direction.W);
                        if (index != -1)
                        {
                            short newX = IndexToCoordinate(index).x;
                            InsertionOrdered(ls3, new Tuple<short, short>(x, newX));
                            x = newX;
                        }
                    } while (index != -1);
                }
            }

            for (short x = 0; x < GridWidth; x++)
            {
                var ls2 = new List<Tuple<short, short>>();
                var ls0 = new List<Tuple<short, short>>();
                jumpLookup[2].Add(ls2);
                jumpLookup[0].Add(ls0);
                for (int yId = 0; yId < yBoundaryPoints[x].Count; yId += 2)
                {
                    short y = yBoundaryPoints[x][yId];
                    int index;
                    do
                    {
                        index = RawJump(new Coordinate(x, y), Direction.N);
                        if (index != -1)
                        {
                            short newY = IndexToCoordinate(index).y;
                            ls2.Add(new Tuple<short, short>(y, newY));
                            y = newY;
                        }
                    } while (index != -1);
                }

                for (int yId = 1; yId < yBoundaryPoints[x].Count; yId += 2)
                {
                    short y = (short) (yBoundaryPoints[x][yId] - 1);
                    int index;
                    do
                    {
                        index = RawJump(new Coordinate(x, y), Direction.S);
                        if (index != -1)
                        {
                            short newY = IndexToCoordinate(index).y;
                            InsertionOrdered(ls0, new Tuple<short, short>(y, newY));
                            y = newY;
                        }
                    } while (index != -1);
                }
            }
        }

        // 刷新 x>=x0,且 x<=x1,和 y>=y0,且 y<=y1 这个区域中的寻路点的缓存,在改变可走性以后调用
        public void ReProcessGrid(short x0, short x1, short y0, short y1)
        {
            if (x0 < 0 || x0 >= GridWidth || x1 < 0 || x1 >= GridWidth)
            {
                throw new Exception($"越界:x0:{x0}, x1:{x1}, y0:{y0} y1:{y1}" +
                                    $" GridWidth:{GridWidth} GridHeight:{GridHeight}");
            }
            if (y0 < 0 || y0 >= GridHeight || y1 < 0 || y1 >= GridHeight)
            {
                throw new Exception($"越界:x0:{x0}, x1:{x1}, y0:{y0} y1:{y1}" +
                                    $" GridWidth:{GridWidth} GridHeight:{GridHeight}");
            }

            for (short y = y0; y <= y1; y++) {
                xBoundaryPoints[y].Clear();
                bool currentPassable = false;
                for (short x = 0; x < GridWidth; x++) {
                    if (IsPassable(x, y) != currentPassable) {
                        xBoundaryPoints[y].Add(x);
                        currentPassable = !currentPassable;
                    }
                }
                xBoundaryPoints[y].Add(GridWidth);
            }

            for (short x = x0; x <= x1; x++) {
                yBoundaryPoints[x].Clear();
                bool currentPassable = false;
                for (short y = 0; y < GridHeight; y++) {
                    if (IsPassable(x, y) != currentPassable) {
                        yBoundaryPoints[x].Add(y);
                        currentPassable = !currentPassable;
                    }
                }
                yBoundaryPoints[x].Add(GridHeight);
            }

            short yStart = (short)(y0 - 1);
            if (yStart < 0)
            {
                yStart = 0;
            }
            short yEnd = (short)(y1 + 1);
            if (yEnd >= GridHeight)
            {
                yEnd = (short)(GridHeight - 1);
            }

            for (short y = yStart; y <= yEnd; y++)
            {
                List<Tuple<short, short>> ls1 = jumpLookup[1][y];
                List<Tuple<short, short>> ls3 = jumpLookup[3][y];
                ls1.Clear();
                ls3.Clear();
                // jumpLookup[1] 是从西到东方向的跳点lookup table,
                for (int xId = 0; xId < xBoundaryPoints[y].Count; xId+=2) {
                    // 下标为偶数元素都是第一个可走点
                    short x = xBoundaryPoints[y][xId];
                    int index;
                    do {
                        // 从第一个可走点向东,可以求出跳点,那么从可走点(x,y),到(newX, y)间的点的跳点都是(newX, y)
                        index = RawJump(new Coordinate(x, y), Direction.E);
                        if (index != -1) {
                            short newX = IndexToCoordinate(index).x;
                            ls1.Add(new Tuple<short, short>(x, newX));
                            x = newX;
                        }
                    } while (index != -1);
                }
                for (int xId = 1; xId < xBoundaryPoints[y].Count; xId += 2) {
                    // 因为:下标为奇数元素都是从西到东的第一个障碍物,由于xBoundaryPoints中边界值必然成对出现,
                    // 所以:第一个障碍物的前一个点,必然是可走点
                    short x = (short) (xBoundaryPoints[y][xId]-1);
                    int index;
                    do {
                        // 从可走点往西可以求出跳点,那么从(newX, y)到(x,y) 间的跳点都是(newX, y)
                        index = RawJump(new Coordinate(x, y), Direction.W);
                        if (index != -1)
                        {
                            short newX = IndexToCoordinate(index).x;
                            InsertionOrdered(ls3, new Tuple<short, short>(x, newX));
                            x = newX;
                        }
                    } while (index != -1);
                }
            }

            short xStart = (short)(x0 - 1);
            if (xStart < 0)
            {
                xStart = 0;
            }
            short xEnd = (short)(x1 + 1);
            if (xEnd >= GridWidth)
            {
                xEnd = (short)(GridWidth - 1);
            }

            for (short x = xStart ; x <= xEnd; x++)
            {
                List<Tuple<short, short>> ls2 = jumpLookup[2][x];
                List<Tuple<short, short>> ls0 = jumpLookup[0][x];
                ls2.Clear();
                ls0.Clear();
                for (int yId = 0; yId < yBoundaryPoints[x].Count; yId += 2)
                {
                    short y = yBoundaryPoints[x][yId];
                    int index;
                    do
                    {
                        index = RawJump(new Coordinate(x, y), Direction.N);
                        if (index != -1)
                        {
                            short newY = IndexToCoordinate(index).y;
                            ls2.Add(new Tuple<short, short>(y, newY));
                            y = newY;
                        }
                    } while (index != -1);
                }

                for (int yId = 1; yId < yBoundaryPoints[x].Count; yId += 2)
                {
                    short y = (short) (yBoundaryPoints[x][yId] - 1);
                    int index;
                    do
                    {
                        index = RawJump(new Coordinate(x, y), Direction.S);
                        if (index != -1)
                        {
                            short newY = IndexToCoordinate(index).y;
                            InsertionOrdered(ls0, new Tuple<short, short>(y, newY));
                            y = newY;
                        }
                    } while (index != -1);
                }
            }
        }

        public void SetCanWalkState(Coordinate co, bool canWalk) {
            var index = GridIndex(co);
            GridData[index] = canWalk;
            // grid.ChangeColor(co, GridColor.Black);
        }

        public void SetPassable(short x, short y, bool canWalk)
        {
            var index = GridIndex(x, y);
            GridData[index] = canWalk;
        }

        private static void InsertionOrdered(List<Tuple<short, short>> ls, Tuple<short, short> p) {
            for (int i = 0; i < ls.Count; i++) {
                if (ls[i].Item2 > p.Item2) {
                    ls.Insert(i, p);
                    return;
                }
            }
            ls.Add(p);
        }

        public int MemorySize() {
            return -1; // 返回 -1 表示没有实现
        }
        public bool HasPreprocessedMap() {
            return preprocessedData;
        }
        private static short version = 1;
        public void WriteDataToFile(BinaryWriter fout) {
            fout.Write(version);
            fout.Write(GridWidth);
            fout.Write(GridHeight);
            byte[] bytes = GridData.ToByteArray(GridHeight, GridWidth);
            fout.Write(bytes);
        }
        public void WritePreDataToFile(BinaryWriter writer) {
            for (int y = 0; y < GridHeight; y++) {
                int numBoundaries = xBoundaryPoints[y].Count;
                writer.Write(numBoundaries);
                for (int i = 0; i < xBoundaryPoints[y].Count; i++) {
                    writer.Write(xBoundaryPoints[y][i]);
                }
            }
            for (int x = 0; x < GridWidth; x++) {
                int numBoundaries = yBoundaryPoints[x].Count;
                writer.Write(numBoundaries);
                for (int i = 0; i < yBoundaryPoints[x].Count; i++) {
                    writer.Write(yBoundaryPoints[x][i]);
                }
            }
            for (int y = 0; y < GridHeight; y++)
            {
                DumpJumpPointData(writer, jumpLookup[1][y]);
                DumpJumpPointData(writer, jumpLookup[3][y]);
            }
            for (int x = 0; x < GridWidth; x++)
            {
                DumpJumpPointData(writer, jumpLookup[2][x]);
                DumpJumpPointData(writer, jumpLookup[0][x]);
            }
        }

        private void DumpJumpPointData(BinaryWriter writer, List<Tuple<short, short>> tupLs) {
            writer.Write(tupLs.Count);
            for (int i = 0; i < tupLs.Count; i++) {
                Tuple<short, short> tup = tupLs[i];
                writer.Write(tup.Item1);
                writer.Write(tup.Item2);
            }
        }

        private void ReadJumpPointData(BinaryReader fin, List<Tuple<short, short>> ls) {
            int count = fin.ReadInt32();
            for (int i = 0; i < count; i++) {
                var item1 = fin.ReadInt16();
                var item2 = fin.ReadInt16();
                ls.Add(new Tuple<short, short>(item1, item2));
            }
        }
        public void ReadDataFromFile(BinaryReader fin) {
            fin.ReadInt16(); // version
            short width = fin.ReadInt16();
            short height = fin.ReadInt16();
            var byteLen = AStarMetrics.BitArrayLength(width, height);
            byte[] bytes = fin.ReadBytes(byteLen);
            BitArray bitArray = new BitArray(bytes);
            for (int i = width*height; i < bitArray.Count; i++) {
                bitArray[i] = false;
            }
            ReInitData(bitArray, width, height);
        }

        public void ReadPreDataFromFile(BinaryReader fin) {
            xBoundaryPoints.Clear();
            for (int y = 0; y < GridHeight; y++) {
                int numBoundaries = fin.ReadInt32();
                var ls = new List<short>(numBoundaries);
                for (int i = 0; i < numBoundaries; i++) {
                    ls.Add(fin.ReadInt16());
                }
                xBoundaryPoints.Add(ls);
            }
            yBoundaryPoints.Clear();
            for (int x = 0; x < GridWidth; x++) {
                int numBoundaries = fin.ReadInt32();
                var ls = new List<short>(numBoundaries);
                for (int i = 0; i < numBoundaries; i++) {
                    ls.Add(fin.ReadInt16());
                }
                yBoundaryPoints.Add(ls);
            }

            for (int i = 0; i < 4; i++) {
                jumpLookup[i] = new List<List<Tuple<short, short>>>();
            }
            for (int y = 0; y < GridHeight; y++) {
                jumpLookup[1].Add(new List<Tuple<short, short>>());
                ReadJumpPointData(fin, jumpLookup[1][y]);
                jumpLookup[3].Add(new List<Tuple<short, short>>());
                ReadJumpPointData(fin, jumpLookup[3][y]);
            }
            for (int x = 0; x < GridWidth; x++) {
                jumpLookup[2].Add(new List<Tuple<short, short>>());
                ReadJumpPointData(fin, jumpLookup[2][x]);
                jumpLookup[0].Add(new List<Tuple<short, short>>());
                ReadJumpPointData(fin, jumpLookup[0][x]);
            }
        }

        #endregion

#if DEBUG_GRID_SHOW
        public SquireGrid grid;
        public void SetGrid(SquireGrid squireGrid) {
            grid = squireGrid;
        }

        #region 调试版FindSolution
        public IEnumerator FindSolution(short sX, short sY, short eX, short eY) {
            List<Coordinate> solution = new List<Coordinate>();
            endX = eX;
            endY = eY;
            endPos = new Coordinate(eX, eY);
            endIndex = GridIndex(eX, eY);
            startPos = new Coordinate(sX, sY);
            startIndex = GridIndex(sX, sY);
            if (sX == eX && sY == eY) {
                yield break;
            }
            if (!InBounds(startIndex) || !InBounds(endIndex) || !IsPassable(startPos) || !IsPassable(endPos)) {
                yield break;
            }

            if (DirectSolution2(sX, sY, eX, eY, ref solution)) {
                foreach (var c in solution) {
                    grid.ChangeColor(c, Color.green);
                }
                yield break;
            }

            GetEndSpaceIds(eX, eY);
            _openListBh.Clear();
            Array.Clear(TestedGrid, 0, TestedGrid.Length);
            _pool.Reset();
            solution.Clear();
            var startNode = _pool.GetNewNode(startPos, endPos, Direction.CENTER, null);
            _openListBh.Insert(startNode);
            SetChecked(GridIndex(startPos));
            DirectionSet dSet = new DirectionSet();
            var waitSecond = new WaitForSeconds(0.3f);
            while (_openListBh.Count() > 0) {
                dSet.Clear();
                Node currentNode = _openListBh.PopMin();
                if (currentNode.pos != startPos) {
                    grid.ChangeColor(currentNode.pos, Color.yellow);
                }
                AddForcedNeighbours(ref dSet, ref currentNode.pos, currentNode.dir);
                AddNaturalNeighbours(ref dSet, currentNode.dir);
                for (Direction dir = Direction.START; dir < Direction.END; dir++) {
                    if (!dSet.IsDirectionSet(dir)) {
                        continue;
                    }
                    int nextIndex = JumpNew(currentNode.pos, dir);
                    if (!InBounds(nextIndex)) {
                        continue;
                    }
                    if (!IsPassable(nextIndex)) {
                        SetChecked(nextIndex);
                        continue;
                    }
                    Coordinate nextC = IndexToCoordinate(nextIndex);
                    if (nextIndex == endIndex) {
                        solution.Add(endPos);
                        Node solutionNode = currentNode;
                        while (solutionNode != null) {
                            solution.Add(solutionNode.pos);
                            solutionNode = solutionNode.parent;
                        }
                        if (solution.Count > 2) {
                            for (int i = 1; i < solution.Count - 1; i++) {
                                var c = solution[i];
                                grid.ChangeColor(c, Color.green);
                            }
                        }
                        _openListBh.Clear();
                        dSet.Clear();
                        yield break;
                    }

                    if (!IsChecked(nextIndex)) {
                        var newNode = _pool.GetNewNode(nextC, endPos, dir, currentNode);
                        grid.ChangeLabel(newNode.pos, newNode.Total);
                        _openListBh.Insert(newNode);
                        yield return waitSecond;
                        SetChecked(nextIndex);
                    }
                    else {
                        Node t = _pool.GetNewNode(nextC, endPos, dir, currentNode);
                        _openListBh.InsertSmaller(t);
                    }
                }
            }
        }
        #endregion
#endif
        bool hasClearPath(short x0, short y0, short x1, short y1) {
            int dx = Math.Abs(x1 - x0);
            int dy = Math.Abs(y1 - y0);
            int x = x0;
            int y = y0;
            int n = 1 + dx + dy;
            int x_inc = (x1 > x0) ? 1 : -1;
            int y_inc = (y1 > y0) ? 1 : -1;
            int error = dx - dy;
            dx *= 2;
            dy *= 2;
            for (; n > 0; n--)
            {
                var co = IndexToCoordinate(GridIndex((short)x, (short)y));
                if (!IsPassable(co))
                {
                    return false;
                }
                if (error > 0)
                {
                    x += x_inc;
                    error -= dy;
                }
                else
                {
                    y += y_inc;
                    error += dx;
                }
            }
            return true;
        }

        public List<int> GetDirectPathTo(short x0, short y0, short x1, short y1)
        {
            var ls = new List<int>();
            int dx = Math.Abs(x1 - x0);
            int dy = Math.Abs(y1 - y0);
            int x = x0;
            int y = y0;
            int n = 1 + dx + dy;
            int x_inc = (x1 > x0) ? 1 : -1;
            int y_inc = (y1 > y0) ? 1 : -1;
            int error = dx - dy;
            dx *= 2;
            dy *= 2;
            for (; n > 0; n--)
            {
                var index = GridIndex((short) x, (short) y);
                if (!IsPassable(index))
                {
                    return ls;
                }
                ls.Add(index);
                if (error > 0)
                {
                    x += x_inc;
                    error -= dy;
                }
                else
                {
                    y += y_inc;
                    error += dx;
                }
            }
            return ls;
        }

        private bool DirectSolution2(short x0, short y0, short x1, short y1, ref List<Coordinate> solution)
        {
            if (hasClearPath(x0, y0, x1, y1))
            {
                solution.Add(new Coordinate(x1, y1));
                solution.Add(new Coordinate(x0, y0));
                return true;
            }
            return false;
        }

        // 因为我们地形比较特殊，产生的路径形状起点和终点更需要
        // 返回值表示是否发生路径简化
        // 如果发生简化，路径为 第二个参数,否则路径为 第一个参数
        public bool SimplifySolutionStart(ref List<Coordinate> sol, ref List<Coordinate> sol2) {
            sol2.Clear();
            var sol_size = sol.Count;
            if (sol_size <= 3) { // 三个点的路径也不需要简化,因为如果能成功,寻路的时候就已经返回直连了!
                return false;
            }
            var second_last_index = sol_size - 2;
            var start_co = sol[0];
            var x0 = start_co.x;
            var y0 = start_co.y;
            // 从倒数第二个到正数第三个
            for (var i = second_last_index; i >= 2; i--) {
                var co = sol[i];
                if(hasClearPath(x0, y0, co.x, co.y)) {
                    sol2.Add(start_co);
                    for (int j = i; j < sol_size; ++j) {
                        sol2.Add(sol[j]);
                    }
                    return true;
                }
            }
            return false;
        }

        // 如果发生简化，路径为 第二个参数,否则路径为 第一个参数
        public bool SimplifySolutionEnd(ref List<Coordinate> sol, ref List<Coordinate> sol2) {
            sol2.Clear();
            var sol_size = sol.Count;
            if (sol_size <= 3) { // 三个点的路径也不需要简化,因为如果能成功,寻路的时候就已经返回直连了!
                return false;
            }
            var second_index = 1;
            var third_last_index = sol_size - 3;
            var end_co = sol[sol_size-1];
            var x1 = end_co.x;
            var y1 = end_co.y;
            // 从正数第二到倒数第三
            for (int i = second_index; i <= third_last_index; ++i) {
                var co = sol[i];
                if(hasClearPath(co.x, co.y, x1, y1)) {
                    for (int j = 0; j <= i; ++j) {
                        sol2.Add(sol[j]);
                    }
                    sol2.Add(end_co);
                    return true;
                }
            }
            return false;
        }

        public string DumpTxtMapData()
        {
            StringBuilder sb = new StringBuilder();
            sb.Append("type octile\n");
            sb.Append($"height {GridHeight}\n");
            sb.Append($"width {GridWidth}\n");
            sb.Append("map\n");
            for (int j = 0; j < GridHeight; j++)
            {
                for (int i = 0; i < GridWidth; i++)
                {
                    if (IsPassable((short) i, (short) j))
                    {
                        sb.Append(".");
                    }
                    else
                    {
                        sb.Append("@");
                    }
                }
                sb.Append("\n");
            }

            return sb.ToString();
        }
    }
}
